//------------------------------------------------------------------------------
//
//  Stinger PWM Drive Service
//
//  Written Jan-2008 by Trevor Taylor, Software Technology, Australia
//
//  Implements the generic Differential Drive contract for the Stinger
//
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Services.SubscriptionManager;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using W3C.Soap;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using dssphttp = Microsoft.Dss.Core.DsspHttp;
using xml = System.Xml;


// For Stinger
//using SerializerSvc = RoboticsConnection.SerializerServices.Proxy;
using PwmMotorProxy = RoboticsConnection.SerializerServices.Motor.Proxy;

using motor = Microsoft.Robotics.Services.Motor.Proxy;
using model = Microsoft.Robotics.PhysicalModel.Proxy;

using coord = Microsoft.Robotics.Services.Coordination.Proxy;

// For HttpGet
using Microsoft.Dss.Core.DsspHttp;
// For HttpStatusCode
using System.Net;


namespace ProMRDS.Robotics.Stinger.PWMDrive
{
    /// <summary>
    /// Provides access to a differential drive (that coordinates two motors that function together).
    /// </summary>
    [DisplayName("Stinger Generic Differential Drive")]
    [Description("Provides access to the Stinger Drive\n(Uses the Generic Differential Drive contract)\n(Partners with Stinger 'brick')")]
    [Contract(Contract.Identifier)]
    [AlternateContract("http://schemas.microsoft.com/robotics/2006/05/drive.html")]
    public class StingerPWMDriveService : DsspServiceBase
    {
        /// <summary>
        /// Embedded XSLT file for formatting the State in a web page
        /// </summary>
        [EmbeddedResource("ProMRDS.Robotics.Stinger.StingerPWMDrive.xslt")]
        string _transform = null;

        /// <summary>
        /// Main Port
        /// </summary>
        /// <remarks>Note: The main port is an instance of the Generic Differential Drive Operations Port</remarks>
        [ServicePort("/stingerpwmdrive", AllowMultipleInstances=false)]
        private drive.DriveOperations _mainPort = new drive.DriveOperations();

        /// <summary>
        /// Microsoft.Robotics.Services.Drive.StingerPWMDrive Partner
        /// </summary>
        [Partner("SubMgr", Contract = "http://schemas.microsoft.com/xw/2005/01/subscriptionmanager.html", CreationPolicy = PartnerCreationPolicy.CreateAlways, Optional = false)]
        private SubscriptionManagerPort _subMgrPort = new Microsoft.Dss.Services.SubscriptionManager.SubscriptionManagerPort();

        /// <summary>
        /// Stinger PWMDrive Service State
        /// </summary>
        /// <remarks>Note: The State is an instance of the Generic Differential Drive State</remarks>
        [InitialStatePartner(Optional=true, ServiceUri="Stinger.PWMDrive.Config.xml")]
        private drive.DriveDifferentialTwoWheelState _state = new drive.DriveDifferentialTwoWheelState();

        // Notice that we only partner with one motor because you can send
        // a request to a motor service with either ID and it will work.
        // There is a trick to this however -- you must send a configuration
        // message to the motor service.
        [Partner("Motor",
            Contract = PwmMotorProxy.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        // Change the policy to UserPartnerListEntry if you want to explicitly
        // specify the partner or the config file
        //  CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry)]
        PwmMotorProxy.MotorOperations _motorPort = new PwmMotorProxy.MotorOperations();
        PwmMotorProxy.MotorOperations _motorNotifyPort = new PwmMotorProxy.MotorOperations();
        // For unsubscribing
        Port<Shutdown> _motorShutdownPort = new Port<Shutdown>();

        // IMPORTANT NOTE:
        // All requests to change the motor power are funnelled through an internal
        // port so that they can be throttled and also so that the different request
        // types all take their turns properly, i.e. SetMotorPower, DriveDistance and
        // RotateDegrees.
        // This is a complex process because interrupting (due to a new request) the
        // DriveDistance and RotateDegrees operations while they are in progress
        // is not simple.

        // Used for driving the motors. Always executes the newest pending drive request.
        private Port<ExecuteMotion> _internalDrivePowerPort = new Port<ExecuteMotion>();

        // Used for stopping the motors after a timed motion
        private Port<StopMotion> _internalStopPort = new Port<StopMotion>();
        // Flag to indicate that a Stop is pending for a timed motion
        bool _ackPending = false;
        // Counter for ACKs (Every ACK has a unique ID)
        int _ackCounter = 0;
        // Current response port (Used to cancel outstanding requests)
        PortSet<DefaultUpdateResponseType, Fault> _ackPort = null;


        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public StingerPWMDriveService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }
        
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            InitializeState();

            base.Start();

            // Wait one time for an InternalDrivePower command
            Activate(Arbiter.ReceiveWithIterator<ExecuteMotion>(false, _internalDrivePowerPort, InternalDrivePowerHandler));

            // Set up the Stop Motion handler which must be in the Exclusive group
            // along with the other drive requests
            MainPortInterleave.CombineWith(
                new Interleave(
                    new ExclusiveReceiverGroup(
                        Arbiter.ReceiveWithIterator<StopMotion>(true, _internalStopPort, StopMotionHandler)
                    ),
                    new ConcurrentReceiverGroup()
                )
            );

            // Initialize the Motor configuration
            // NOTE: If you don't do this, the PowerScalingFactor is zero.
            // That means the motors never turn on!
            PwmMotorProxy.SetConfiguration config = new PwmMotorProxy.SetConfiguration();
            config.Body.PowerScalingFactor = 1.0;
            config.Body.ReversePolarity = false;
            _motorPort.SetConfiguration(config.Body);

            // Send a motor command so that the serial port gets initialized,
            // otherwise the first command might not function as expected
            SetMotors(0, 0);

        }

        #region Initialization

        /// <summary>
        /// Initialize the drive state
        /// </summary>
        private void InitializeState()
        {
            // Create a new state if there is none
            if (_state == null)
                _state = new drive.DriveDifferentialTwoWheelState();

            // Sanity check (or reset) all of the state properties

            _state.DriveState = drive.DriveState.Stopped;
            _state.IsEnabled = false;

            // The distance between wheels is not exact because the
            // actual contact points of the wheels with the ground
            // might not be in the middle of the wheels and the wheels
            // are very fat!
            // The distance is about 22cm from center to center.
            if (_state.DistanceBetweenWheels == 0.0)
                _state.DistanceBetweenWheels = 0.22;

            if (_state.LeftWheel == null)
                _state.LeftWheel = new motor.WheeledMotorState();
            if (_state.RightWheel == null)
                _state.RightWheel = new motor.WheeledMotorState();

            _state.LeftWheel.WheelSpeed = 0.0;
            _state.RightWheel.WheelSpeed = 0.0;

            if (_state.LeftWheel.GearRatio == 0.0)
                _state.LeftWheel.GearRatio = 1.0;
            if (_state.RightWheel.GearRatio == 0.0)
                _state.RightWheel.GearRatio = 1.0;

            // Wheel radius on the Stinger is tiny!
            // Only about 2.75cm, but this cannot be measured very
            // accurately and there will also be effects as the
            // wheels sink into carpet, etc. And when you add
            // weight to the robot the wheels compress because
            // they are soft.
            if (_state.LeftWheel.Radius == 0.0)
                _state.LeftWheel.Radius = 0.0275;
            if (_state.RightWheel.Radius == 0.0)
                _state.RightWheel.Radius = 0.0275;

            model.Pose p = new model.Pose();
            p.Position = new model.Vector3(-0.09f, 0.0275f, 0);
            p.Orientation = new model.Quaternion(-1, 0, 0, 0);
            if (_state.LeftWheel.MotorState == null)
                _state.LeftWheel.MotorState = new motor.MotorState("Left Motor", 1, 1.0, false, p);
            if (_state.LeftWheel.MotorState.HardwareIdentifier == 0)
                _state.LeftWheel.MotorState.HardwareIdentifier = 1;
            if (string.IsNullOrEmpty(_state.LeftWheel.MotorState.Name))
                _state.LeftWheel.MotorState.Name = "Left Motor";
            if (_state.LeftWheel.MotorState.PowerScalingFactor == 0.0)
                _state.LeftWheel.MotorState.PowerScalingFactor = 1.0;

            p.Position = new model.Vector3(0.09f, 0.0275f, 0);
            p.Orientation = new model.Quaternion(1, 0, 0, 0);
            if (_state.RightWheel.MotorState == null)
                _state.RightWheel.MotorState = new motor.MotorState("Right Motor", 2, 1.0, true, p);
            if (_state.RightWheel.MotorState.HardwareIdentifier == 0)
                _state.RightWheel.MotorState.HardwareIdentifier = 2;
            if (string.IsNullOrEmpty(_state.RightWheel.MotorState.Name))
                _state.RightWheel.MotorState.Name = "Right Motor";
            if (_state.RightWheel.MotorState.PowerScalingFactor == 0.0)
                _state.RightWheel.MotorState.PowerScalingFactor = 1.0;

            SaveState(_state);
        }

        #endregion

        #region Operation Handlers

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(drive.Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }
        
        /// <summary>
        /// HttpGet Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(Microsoft.Dss.Core.DsspHttp.HttpGet httpGet)
        {
            // Replace the default code so that a transform can be used
            // get.ResponsePort.Post(new dssphttp.HttpResponseType(_state));
            httpGet.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _state,
                _transform)
            );
            yield break;
        }
        
        /// <summary>
        /// HttpPost Handler
        /// </summary>
        /// <param name="submit"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpPostHandler(dssphttp.HttpPost submit)
        {
            // TODO: Implement Submit operations here.
            throw new NotImplementedException("TODO: Implement Submit operations here.");
        }
        
        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReliableSubscribeHandler(drive.ReliableSubscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }
        
        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SubscribeHandler(drive.Subscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }
        
        /// <summary>
        /// Update Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> UpdateHandler(drive.Update update)
        {
            // TODO: Implement Update operations here.
            throw new NotImplementedException("TODO: Implement Update operations here.");
        }
        
        /// <summary>
        /// EnableDrive Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> EnableDriveHandler(drive.EnableDrive update)
        {
            // Stop now if drive is to be disabled
            if (!update.Body.Enable)
            {
                ClearPendingStop();
                // Internal drive power is not affected by Enable
                drive.SetDrivePower sdp = new drive.SetDrivePower();
                sdp.Body.LeftWheelPower = 0;
                sdp.Body.RightWheelPower = 0;
                _internalDrivePowerPort.Post(new ExecuteMotion(Motions.SetDrivePower, sdp));
                _state.DriveState = drive.DriveState.Stopped;
            }

            _state.IsEnabled = update.Body.Enable;
            _state.TimeStamp = DateTime.Now;
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }
        
        /// <summary>
        /// SetDrivePower Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower update)
        {
            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Drive is not enabled")));
                yield break;
            }

            // All motion requests must do this first!
            ClearPendingStop();

            Port<drive.SetDrivePower> p = _mainPort;
            if (p.ItemCount > 10)
                Console.WriteLine("Backlog of " + p.ItemCount);

            // Set the drive state appropriately (in anticipation)
            if (update.Body.LeftWheelPower == 0 && update.Body.RightWheelPower == 0)
                _state.DriveState = drive.DriveState.Stopped;
            else
                _state.DriveState = drive.DriveState.DrivePower;

            // Pass the request to the internal handler
            _internalDrivePowerPort.Post(new ExecuteMotion(Motions.SetDrivePower, update));

        }
        
        /// <summary>
        /// SetDriveSpeed Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetDriveSpeedHandler(drive.SetDriveSpeed update)
        {
            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Drive is not enabled")));
                yield break;
            }

            // All motion requests must do this first!
            ClearPendingStop();

            // TODO: Convert meters/second to Hemisson power based on radius of wheels
            // There has to be another "fudge factor" that converts m/s to a power setting
            // Many of the available services do not implement speed because it is too hard!

            // Force this via the internal port so that it gets queued
            // with the rest of the drive requests
            drive.SetDrivePower power = new drive.SetDrivePower();
            // Just copy speed across to power
            // Obviously this is NOT correct, but whatcha gonna do?
            power.Body.LeftWheelPower = update.Body.LeftWheelSpeed;
            power.Body.RightWheelPower = update.Body.RightWheelSpeed;
            // Copy the response port too so that the sender will get a reply
            power.ResponsePort = update.ResponsePort;

            if (power.Body.LeftWheelPower == 0 && power.Body.RightWheelPower == 0)
                _state.DriveState = drive.DriveState.Stopped;
            else
                _state.DriveState = drive.DriveState.DriveSpeed;

            // Use the internal handler for throttling
            _internalDrivePowerPort.Post(new ExecuteMotion(Motions.SetDrivePower, power));

        }

        #region Timed Moves

        // NOTE:
        // This is tacky hack. When using the PWM drive, the wheel encoders are
        // not used. So to make the robot move a specified distance or turn by
        // a specified angle we make an educated guess as to the amount of time
        // it needs to move for. Obviously this is not accurate! Even worse, the
        // TimeoutPort under MRDS is nowhere near accurate. Consequently these
        // operations are implemented just so that something happens rather than
        // giving a "Not Implemented" exception.

        /// <summary>
        /// CalculateTimeToMove
        /// </summary>
        /// <param name="distance">Distance to travel</param>
        /// <param name="power">Drive power to use</param>
        /// <returns>Number of milliseconds</returns>
        public int CalculateTimeToMove(double distance, double power)
        {
            // Drive distance is in meters so for now just convert to milliseconds
            // (which is equivalent to "millimeters" in the Dashboard user interface)
            //  Notice that the power is not used at all (yet)!
            //return (int)(Math.Abs(distance) * 1000.0);
            // First guess -- Times 6
            return (int)(Math.Abs(distance) * 5550.0);
        }


        // Issue a fault if a pending Stop was terminated and
        // clear the flag
        void ClearPendingStop()
        {
            if (_ackPending)
            {
                _ackPending = false;
                Fault f;
                f = Fault.FromException(new Exception("Stop after timed motion aborted"));
                _ackPort.Post(f);
                _ackPort = null;
            }
        }

        // Set a timer to post a Stop later
        void SetStopTimer(int delay, PortSet<DefaultUpdateResponseType, Fault> p)
        {
            int num;

            // Classic case of "should never happen"
            ClearPendingStop();

            // Increment the ACK counter to make it unique and
            // keep a copy for later
            _ackCounter++;
            num = _ackCounter;
            // Remember the current response port in case it has
            // to be cancelled
            _ackPort = p;
            // Enable ACK
            _ackPending = true;

            // Setup a delegate for the specified time period
            // It will post a StopMotion message when it fires
            Activate(Arbiter.Receive(
                false,
                TimeoutPort(delay),
                delegate(DateTime timeout)
                {
                    // Send ourselves a Stop request using the ACK number
                    _internalStopPort.PostUnknownType(new StopMotion(num));
                }
            ));
        }


        // Stop Motion Handler
        // Called in response to a Stop message when a timed motion finishes
        public virtual IEnumerator<ITask> StopMotionHandler(StopMotion stop)
        {
            // Somebody beat us to it!
            if (!_ackPending)
                yield break;

            // Another request has arrived and overridden this one
            // so there is still one pending, but it is not us
            if (stop.Body.AckNumber != _ackCounter)
                yield break;

            // Now stop the robot and post a response back to say the move is complete
            drive.SetDrivePower power = new drive.SetDrivePower();
            // Set the power
            power.Body.LeftWheelPower = 0;
            power.Body.RightWheelPower = 0;
            // Set the response port
            power.ResponsePort = _ackPort;

            // Use the internal handler for throttling
            _internalDrivePowerPort.Post(new ExecuteMotion(Motions.SetDrivePower, power));
            _state.DriveState = drive.DriveState.Stopped;

            /*
            yield return Arbiter.Choice(_brickPort.UpdateMotorPower(0.0, 0.0),
                delegate(DefaultUpdateResponseType ok)
                {
                    // Note that the response is after the move has finished
                    _ackPort.Post(ok);
                },
                delegate(Fault fault)
                {
                    LogError("Timed Motion - Could not stop motors");
                    stop.ResponsePort.Post(fault);
                });
            */

            // Clear the ACK
            _ackPending = false;
            _ackPort = null;

        }

        /// <summary>
        /// RotateDegrees Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> RotateDegreesHandler(drive.RotateDegrees update)
        {
            // This handler is basically the same as DriveDistance except for the
            // calculation of the distance. It could use DriveDistance, but that
            // is an unnecessary overhead.
            // See DriveDistance for more comments.

            // Time delay for the motion
            int delay;
            // Speeds for wheels to move at
            double leftSpeed;
            double rightSpeed;
            // Distance wheels will travel during rotation
            double arcDistance;

            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Drive is not enabled")));
                yield break;
            }

            // All motion requests must do this first!
            ClearPendingStop();

            // Check for zero degrees first because we don't want the wheels to jerk
            // if the robot is supposed to go nowhere!
            if (update.Body.Degrees == 0.0)
            {
                update.ResponsePort.Post(new DefaultUpdateResponseType());
                yield break;
            }
            else if (update.Body.Degrees < 0.0)
            {
                // Note that the speeds are opposite to make the robot
                // rotate on the spot
                leftSpeed = update.Body.Power;
                rightSpeed = -update.Body.Power;
            }
            else
            {
                leftSpeed = -update.Body.Power;
                rightSpeed = update.Body.Power;
            }

            // Calculate the distance that the wheels must travel for the given
            // turn angle. They will move around the circumference of a circle
            // with a diameter equal to the wheel base. Each 360 degrees of rotation
            // is one full circumference.
            // For the supposed wheelbase of 10.5cm this is 32.98672cm
            arcDistance = Math.Abs(update.Body.Degrees) * _state.DistanceBetweenWheels * Math.PI / 360;

            // Calculate the delay time
            delay = CalculateTimeToMove(arcDistance, update.Body.Power);

            // Start the motors running (in opposite directions)
            drive.SetDrivePower power = new drive.SetDrivePower();
            // Set the power
            power.Body.LeftWheelPower = leftSpeed;
            power.Body.RightWheelPower = rightSpeed;
            // Do NOT respond to this message (Happens later on Stop)
            power.ResponsePort = null;

            // Use the internal handler for throttling
            _internalDrivePowerPort.Post(new ExecuteMotion(Motions.RotateDegrees, power));
            _state.DriveState = drive.DriveState.RotateDegrees;

            // Set the timer and forget about it
            SetStopTimer(delay, update.ResponsePort);

        }
        
        /// <summary>
        /// DriveDistance Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> DriveDistanceHandler(drive.DriveDistance update)
        {
            // Drive Distance can be used to move a specified distance.
            //
            // Use a timer -- start the motors, wait, stop the motors.
            //
            // The problem is that using timers on Windows is not
            // reliable. On top of that, there are transmission delays
            // when sending commands, to it is not possible to make very
            // accurate moves this way.
            //

            // Time delay for the motion
            int delay;
            // Speed to move at
            // NOTE: "speed" is really the power because robot does not implement both.
            // Power values should be between 0 and 1.
            // The speed should be calculated based on the wheel radius and power,
            // but this is not implemented.
            double speed = Math.Abs(update.Body.Power);
            // Distance to travel
            // NOTE: Microsoft says that the Distance should always be POSITIVE and
            // that you use a negative Power to move backwards. I believe that a
            // convention of negative distances should be acceptable. Therefore
            // the code therefore checks both possibilities. This leads to the
            // strange case of a negative distance AND power, this is forwards!
            double distance = update.Body.Distance * Math.Sign(update.Body.Power);

            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Drive is not enabled")));
                yield break;
            }

            // All motion requests must do this first!
            ClearPendingStop();

            // Check for zero distance first because we don't want the wheels to jerk
            // if the robot is supposed to go nowhere!
            if (distance == 0.0)
            {
                update.ResponsePort.Post(new DefaultUpdateResponseType());
                yield break;
            }
            else if (distance < 0.0)
            {
                // Speed needs to be negative if distance is negative
                speed = -speed;
            }
            else
            {
                // Speed remains the same (positive)
            }

            // Calculate the delay time
            delay = CalculateTimeToMove(distance, speed);

            // Start the motors running!
            drive.SetDrivePower power = new drive.SetDrivePower();
            // Set the power
            power.Body.LeftWheelPower = speed;
            power.Body.RightWheelPower = speed;
            // Do NOT respond to this message (Happens later on Stop)
            power.ResponsePort = null;

            // Use the internal handler for throttling
            _internalDrivePowerPort.Post(new ExecuteMotion(Motions.DriveDistance, power));
            _state.DriveState = drive.DriveState.DriveDistance;

            // Set the timer and forget about it
            SetStopTimer(delay, update.ResponsePort);

        }

        #endregion

        /// <summary>
        /// AllStop Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> AllStopHandler(drive.AllStop update)
        {
            // All motion requests must do this first!
            ClearPendingStop();

            // Kill all outstanding requests!
            // Because this is Exclusive, there cannot be another drive operation
            // in progress
            //_internalDrivePowerPort.Clear();
            int pendingCommands = _internalDrivePowerPort.ItemCount;

            // If newer commands have been issued, send success responses
            // to the older commands and move to the latest one
            //drive.SetDrivePower newerPower;
            ExecuteMotion newerUpdate;
            while (pendingCommands > 0)
            {
                if (_internalDrivePowerPort.Test(out newerUpdate))
                {
                    // Timed motion requests do not have a response initially,
                    // that comes later when the motors are stopped
                    if (newerUpdate.Body.Motion == Motions.SetDrivePower)
                    {
                        if (newerUpdate.Body.SetPower.ResponsePort != null)
                            newerUpdate.Body.SetPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                    }
                }
                pendingCommands--;
            }

            _state.DriveState = drive.DriveState.Stopped;

            // Explicitly send a stop command
            /*
            yield return Arbiter.Choice(_brickPort.UpdateMotorPower(0.0, 0.0),
                delegate(DefaultUpdateResponseType ok)
                {
                    update.ResponsePort.Post(ok);
                },
                delegate(Fault fault)
                {
                    update.ResponsePort.Post(fault);
                }
            );
             */

            SetMotors(0, 0);

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;

        }

        #endregion

        #region Internal Handlers

        /// <summary>
        /// Process the most recent Drive Power command
        /// When complete, self activate for the next internal command
        /// </summary>
        /// <param name="driveDistance"></param>
        /// <returns></returns>
        public virtual IEnumerator<ITask> InternalDrivePowerHandler(ExecuteMotion exec)
        {
            drive.SetDrivePower power = exec.Body.SetPower;
            try
            {
                // Take a snapshot of the number of pending commands at the time
                // we entered this routine.
                // This will prevent a livelock which can occur if we try to
                // process the queue until it is empty, but the inbound queue
                // is growing at the same rate as we are pulling off the queue.
                int pendingCommands = _internalDrivePowerPort.ItemCount;

                // If newer commands have been issued, send success responses
                // to the older commands and move to the latest one
                //drive.SetDrivePower newerPower;
                ExecuteMotion newerUpdate;
                while (pendingCommands > 0)
                {
                    if (_internalDrivePowerPort.Test(out newerUpdate))
                    {
                        // Timed motion requests do not have a response initially,
                        // that comes later when the motors are stopped
                        if (newerUpdate.Body.Motion == Motions.SetDrivePower)
                        {
                            if (newerUpdate.Body.SetPower.ResponsePort != null)
                                newerUpdate.Body.SetPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                        }
                        exec = newerUpdate;
                        power = exec.Body.SetPower;
                    }
                    pendingCommands--;
                }

                /*
                yield return Arbiter.Choice(_brickPort.UpdateMotorPower(power.Body.LeftWheelPower, power.Body.RightWheelPower),
                    delegate(DefaultUpdateResponseType ok)
                    {
                        _state.LeftWheel.MotorState.CurrentPower = power.Body.LeftWheelPower;
                        _state.RightWheel.MotorState.CurrentPower = power.Body.RightWheelPower;
                        if (power.ResponsePort != null)
                            power.ResponsePort.Post(ok);
                    },
                    delegate(Fault fault)
                    {
                        if (power.ResponsePort != null)
                            power.ResponsePort.Post(fault);
                    }
                );
                 */

                // Now actually set the motors
                SetMotors(power.Body.LeftWheelPower, power.Body.RightWheelPower);
                // Post back a response
                if (exec.Body.Motion == Motions.SetDrivePower)
                    exec.Body.SetPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            }
            finally
            {
                // Wait one time for the next InternalDrivePower command
                Activate(Arbiter.ReceiveWithIterator(false, _internalDrivePowerPort, InternalDrivePowerHandler));
            }

            yield break;
        }

        // Common routine to handle all motor commands
        // NOTE: This should not be called directly. Post a MotionCommand
        // to the main port so that the requests will be queued (it is an
        // Exclusive handler). There are a couple of places where this rule
        // is violated because it is safe to do so.
        void SetMotors(double leftPower, double rightPower)
        {
            int motor1Power = (int)(leftPower * 100);
            int motor2Power = (int)(rightPower * 100);

            // Add a coordination header to the motor requests
            // so that advanced motor implementations can
            // coordinate the individual motor reqests.
            // Note that there are TWO items in the coordination.
            coord.ActuatorCoordination coordination = new coord.ActuatorCoordination();
            coordination.Count = 2;

            // Set up the requests
            PwmMotorProxy.SetMotorPowerRequest req1 = new PwmMotorProxy.SetMotorPowerRequest();
            req1.Id = 1;
            req1.TargetPower = motor1Power;
            PwmMotorProxy.SetMotorPower leftSmp = new PwmMotorProxy.SetMotorPower(req1);
            leftSmp.AddHeader(coordination);

            PwmMotorProxy.SetMotorPowerRequest req2 = new PwmMotorProxy.SetMotorPowerRequest();
            req2.Id = 2;
            req2.TargetPower = motor2Power;
            PwmMotorProxy.SetMotorPower rightSmp = new PwmMotorProxy.SetMotorPower(req2);
            rightSmp.AddHeader(coordination);

            Console.WriteLine("Motor Power: " + motor1Power + "," + motor2Power);

            // Update the state so the information is visible
            _state.LeftWheel.MotorState.CurrentPower = leftPower;
            _state.RightWheel.MotorState.CurrentPower = rightPower;

            // Now post to both motors as quickly as possible just in case
            // the coordination does not work
            _motorPort.Post(leftSmp);
            _motorPort.Post(rightSmp);

        }

    #endregion

    }
}
